#ifndef __APP_TENTACLE_H__
#define __APP_TENTACLE_H__

#include "stdint.h"
#include "app_pid.h"

// 触角控制相关定义
#define TENTACLE_ENABLE             1
#define TENTACLE_MAX_LINE_LENGTH    500.0f    // 最大线长(mm)
#define TENTACLE_MIN_LINE_LENGTH    50.0f     // 最小线长(mm)
#define TENTACLE_PULSE_TO_MM        0.1f      // 编码器脉冲到毫米的转换系数

// 多节触角参数
#define TENTACLE_SEGMENTS           5         // 触角节数
#define TENTACLE_SEGMENT_LENGTH     20.0f     // 每节长度 (mm)
#define TENTACLE_TOTAL_LENGTH       (TENTACLE_SEGMENTS * TENTACLE_SEGMENT_LENGTH)  // 总长度 (mm)
#define TENTACLE_TAPER_RATIO        0.8f      // 锥度比 (每节半径递减比例)

// 三根钢丝的锚点坐标 (mm) - 在圆锥形触角底部圆周上120度均匀分布
// 假设触角底部半径为R，锚点分布在底部圆周上
#define TENTACLE_BASE_RADIUS    25.0f    // 触角底部半径 (mm)

#define ANCHOR1_X    25.0f      // 0度位置
#define ANCHOR1_Y    0.0f
#define ANCHOR1_Z    0.0f

#define ANCHOR2_X    -12.5f     // 120度位置
#define ANCHOR2_Y    21.65f
#define ANCHOR2_Z    0.0f

#define ANCHOR3_X    -12.5f     // 240度位置
#define ANCHOR3_Y    -21.65f
#define ANCHOR3_Z    0.0f

// 触角控制模式
typedef enum {
    TENTACLE_MODE_POSITION = 0,    // 位置控制模式
    TENTACLE_MODE_TRAJECTORY,      // 轨迹跟踪模式
    TENTACLE_MODE_CIRCLE,          // 圆形运动模式
    TENTACLE_MODE_SPIRAL,          // 螺旋运动模式
    TENTACLE_MODE_MAX
} tentacle_mode_t;

// 触角节结构
typedef struct {
    float x, y, z;                 // 节的位置 (mm)
    float radius;                  // 节的半径 (mm)
    float angle_x, angle_y;        // 节的倾斜角度 (弧度)
} tentacle_segment_t;

// 触角位置结构
typedef struct {
    float x, y, z;                 // 触角末端位置 (mm)
    float line1_length;            // 线1长度 (mm)
    float line2_length;            // 线2长度 (mm)
    float line3_length;            // 线3长度 (mm)
    tentacle_segment_t segments[TENTACLE_SEGMENTS];  // 各节的位置和姿态
} tentacle_position_t;

// 触角控制结构
typedef struct {
    tentacle_position_t target;    // 目标位置
    tentacle_position_t current;   // 当前位置
    tentacle_mode_t mode;          // 控制模式
    uint8_t enable;               // 使能标志
    float max_speed;              // 最大运动速度 (mm/s)
    float trajectory_time;        // 轨迹时间 (s)
    float circle_radius;          // 圆形运动半径 (mm)
    float spiral_radius;          // 螺旋运动半径 (mm)
    float spiral_height;          // 螺旋运动高度 (mm)
} tentacle_control_t;

// 轨迹点结构
typedef struct {
    float x, y, z;                // 位置
    float time;                   // 时间点
} trajectory_point_t;

// 全局触角控制变量
extern tentacle_control_t g_tentacle_ctrl;

// 触角控制函数声明
void Tentacle_Init(void);
void Tentacle_Set_Target_Position(float x, float y, float z);
void Tentacle_Set_Mode(tentacle_mode_t mode);
void Tentacle_Enable(uint8_t enable);
void Tentacle_Stop(void);

// 运动学函数
void Tentacle_Forward_Kinematics(float line1_len, float line2_len, float line3_len, 
                                 float* x, float* y, float* z);
void Tentacle_Inverse_Kinematics(float x, float y, float z, 
                                 float* line1_len, float* line2_len, float* line3_len);

// 多节触角运动学函数
void Tentacle_Calculate_Segments(float line1_len, float line2_len, float line3_len,
                                 tentacle_segment_t* segments);
void Tentacle_Update_Segment_Positions(tentacle_segment_t* segments);
float Tentacle_Get_Segment_Radius(int segment_index);

// 轨迹生成函数
void Tentacle_Generate_Circle_Trajectory(float radius, float height, int num_points, 
                                         trajectory_point_t* trajectory);
void Tentacle_Generate_Spiral_Trajectory(float radius, float height, int turns, int num_points, 
                                         trajectory_point_t* trajectory);
void Tentacle_Generate_Line_Trajectory(float start_x, float start_y, float start_z,
                                       float end_x, float end_y, float end_z, int num_points,
                                       trajectory_point_t* trajectory);

// 控制函数
void Tentacle_Control_Handle(void);
void Tentacle_Position_Control(void);
void Tentacle_Trajectory_Control(void);

// 工具函数
float Tentacle_Calculate_Distance(float x1, float y1, float z1, float x2, float y2, float z2);
void Tentacle_Update_Current_Position(void);
uint8_t Tentacle_Is_Position_Reachable(float x, float y, float z);

#endif /* __APP_TENTACLE_H__ */
